/*
* LOL
* FakeMotors ROS lab
*
* Solution coded by Group 9, 2012-09-25.
* 
* The program attempts to keep the robot on track by keeping track of each wheels
* tick count at the start of a movement and then halting a wheel if it runs ahead of the other.
*
* NOTE: The program does not do any error checing of input, so please keep within bounds.
* 
*/
#include "MotorControl.h"

//All global variables, whatever works bro!
ros::Subscriber	enc_sub;
ros::Publisher	mot_pub;
ros::Publisher	int_pub;
ros::Subscriber mot_sub;
ros::Publisher inter_Pub;
Motor motor; //Fult
int l, r; //This would be the respective wheels CURRENT tick count.
int command;
double dist_or_angle = 0;
float speed = STRAIGHT_SPEED;
double changeL = 1.0;
double changeR = 1.0;

int received_new_command;
double new_dist_or_angle = 0;
/*
Callback function for the "/encoder" topic. Prints the enconder value
and updates l and r. Nothing more.
*/
void recive_encoder(const Encoder::ConstPtr &msg){
	r = msg->right;
	l = msg->left;
	//printf("%f:got encoder L:%i , false R:%i\n",msg->timestamp,l,r);
}

void recive_change(const saint_nr_9::MotCorrect::ConstPtr &correct)
{ 
  changeL = 1.0-correct->right;
  changeR = 1.0-correct->left;
}

void recive_command(const saint_nr_9::MotCom::ConstPtr &msg){
  //printf("Got command %d\n", msg->command);
	received_new_command = msg->command;
	new_dist_or_angle = (double)msg->dist_or_angle;
	
	if(fabs(msg->speed) < 1.0 )
	{
	  //printf("Nice speed: %f\n",msg->speed);
	  speed = msg->speed;
	}
	else
	{
	  speed = STRAIGHT_SPEED;
	}
}

/*
Main. No parameters.
*/
int main(int argc, char **argv){
	//St stuff up
	ros::init(argc, argv, "MotorControl");//Creates a node named "MotorControl"
	ros::NodeHandle n;
	enc_sub = n.subscribe("/serial/encoder", 1, recive_encoder);//when "/encoder" topic is revieved call recive_encoder function
	ros::Subscriber change_sub = n.subscribe("/control/correct_motor", 1, recive_change);
	mot_sub = n.subscribe("/control/motor_control", 1, recive_command);
	mot_pub = n.advertise<Motor>("/serial/motor_speed", 1);//used to publish a topic that changes the motorspeed
	int_pub = n.advertise<std_msgs::Int32>("/serial/encoder_interval", 1);//used to publish a topic that changes the intervall between the "/encoder" topics published.
  inter_Pub = n.advertise<std_msgs::Int32>("Signals/state_motor", 1);      
	std_msgs::Int32 msg;
  msg.data = 100;
	ros::Rate loop_rate(10);
	int_pub.publish(msg);
	int sl, sr; 
	l = 0;
	r = 0;
	command = -1;
	
	//OK, lets go!
	while(ros::ok()){
		//int_pub.publish(msg);		
		//This would be the respective wheels tick count, AT THE START OF A MOVEMENT.
		//Update sl, sr
		sl = l;
		sr = r;
		//printf("Motor Command: %d\n", command);
		//This should be pretty self explanatory...
		if(command == MOVE_FORWARD_DIST) //FORWARD
		{
			forward(dist_or_angle, sl, sr);
			command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
		else if(command == MOVE_BACKWARD_DIST) //BACKWARD
		{
			backward(dist_or_angle, sl, sr);
			command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
		else if(command == TURN_LEFT) //LEFT
		{
			t_left(dist_or_angle, sl, sr);
			command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
		else if(command == TURN_RIGHT) //RIGHT
		{
			t_right(dist_or_angle, sl, sr);
			command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
		else if(command == MOVE_FORWARD)
		{
			forward_infinity(sl,sr);
			command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
		else if(command == MOVE_FORWARD_CORRECT)
		{
		  forward_easy_correct(sl,sr);
		  command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
		else if (command == STOP)
		{
		  stop(sl, sr);
		  command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
		else
		{
		  command = received_new_command;
		  dist_or_angle = new_dist_or_angle;
		  received_new_command = -1;
      new_dist_or_angle = 0;
		}
    
    
    ros::spinOnce();
    loop_rate.sleep();
	}
	
	
	motor.right = 0;
	motor.left = 0;
	mot_pub.publish(motor);
	return 0;
}

/*
Just returns the distance between numbers a and b on the number line.
Works for [-inf, inf].

I made this into a function because its used so much in the program.
*/
int dist(int a , int b){
	if(a >= 0 && b >= 0)
		return abs(a-b);
	if(a >= 0 && b < 0)
		return abs(a-b);
	if(a <= 0 && b < 0)
		return abs(a-b);
	if(a <= 0 && b >= 0)
		return abs(a)+b;
		
	printf("MOTOR_CONTROl: Method diff error\n");	
	return -1; //How the hell did we get here???
}

void stop(int sl, int sr)
{
  motor.right = 0;
	motor.left = 0;
	mot_pub.publish(motor); 
	for (int i = 0; i < 10; i++)
	{
	  mot_pub.publish(motor); 
	  usleep(1);
	  ros::spinOnce();
	}
 /* while(dist(l,sl) > 0)
  {
    mot_pub.publish(motor); 
    sl = l;
    sr = r;
    ros::spinOnce();
	  loop.sleep();
   }*/
}

//This command breaks when command is updated
void forward_infinity(int sl, int sr){
		motor.right = -speed;
		motor.left = -speed;
		mot_pub.publish(motor); //Publish...
		
		ros::Rate loop(100);

		printf("START FORWARD INF...\n");
		while((received_new_command == -1 || received_new_command == MOVE_FORWARD) && ros::ok()){  //263 ticks ~= 25 cm
			if(dist(l, sl) != dist(r, sr)){ //If the wheel are not in synch...
				if(dist(l, sl) > dist(r, sr)){ //... Then depending on which wheel is runing too fast, halt it.
					motor.left = -speed;
					motor.right = -0.95*speed;
				}
				else{
					motor.left = -0.95*speed;
					motor.right = -speed;
					//printf("Halt LEFT\n");
				} 
			}

			mot_pub.publish(motor);

			ros::spinOnce();
			loop.sleep();
		}
		
		//Halt
		motor.right = 0;
		motor.left = 0;
		mot_pub.publish(motor); //Publish...	
}

void forward_easy_correct(int sl, int sr)
{
  motor.right = -speed;
  motor.left = -speed;
  mot_pub.publish(motor); //Publish...

  ros::Rate loop(100);

  printf("START FORWARD GOOD...\n");
  while((received_new_command ==-1 || received_new_command == MOVE_FORWARD_CORRECT) && ros::ok())
  {
    //if(dist(sl, l) > dist(sr, r))
      motor.left = -changeL*speed;
    //else
      motor.right = -changeR*speed;
      
    mot_pub.publish(motor);
    
    ros::spinOnce();
	  loop.sleep();
  }
  motor.right = 0;
	motor.left = 0;
	mot_pub.publish(motor);

  ros::spinOnce();
  loop.sleep();
}



/*
Move forward distance distance, sl and sr are the start-tick-counts 
of respective wheel.
*/
void backward(double distance, int sl, int sr){
		motor.right = STRAIGHT_SPEED;
		motor.left = STRAIGHT_SPEED;
		mot_pub.publish(motor); //Publish...
		
		ros::Rate loop(100);

		printf("START FORWARD...\n");
		while((dist(l, sl) + dist(r, sr))/2 < distance/METERS_P_TICKS)
		{  //263 ticks ~= 25 cm	
			if(dist(l, sl) != dist(r, sr)){ //If the wheel are not in synch...
				if(dist(l, sl) > dist(r, sr)){ //... Then depending on which wheel is runing too fast, halt it.
					motor.left = STRAIGHT_SPEED;
					motor.right = 0.95*STRAIGHT_SPEED;
				}
				else{
					motor.left = 0.95*STRAIGHT_SPEED;
					motor.right = STRAIGHT_SPEED;
				}
			}

			mot_pub.publish(motor);

			ros::spinOnce();
			loop.sleep();
		}
		//if (command == MOVE_BACKWARD_DIST)
		 // command = -1;
		printf("STOP\n");
    std_msgs::Int32 pub;
    pub.data = 1;
    inter_Pub.publish(pub);
		//Halt
		motor.right = 0;
		motor.left = 0;
		for (int i = 0; i < 5; i++)
		{
  		mot_pub.publish(motor); //Publish...
  		ros::spinOnce();
  		}
		
}

/* 
This function works exactly like 'forward' but, suprise, moves backward.
*/
void forward(double distance, int sl, int sr){
	motor.right = -STRAIGHT_SPEED;
	motor.left = -STRAIGHT_SPEED;
	mot_pub.publish(motor);
	
	ros::Rate loop(100);

	printf("START BACKWARD...\n");
	while((dist(l, sl) + dist(r, sr))/2 < distance/METERS_P_TICKS)
  {
		if(dist(l, sl) != dist(r, sr)){
			if(dist(l, sl) > dist(r, sr)){
				motor.left = -STRAIGHT_SPEED;
				motor.right = 0.95*-STRAIGHT_SPEED;
			}
			else{
				motor.left = 0.95*-STRAIGHT_SPEED;
				motor.right = -STRAIGHT_SPEED;
			} 
		}
		
		//motor.right = motor.right * 0.98;
		mot_pub.publish(motor);
	
		loop.sleep();
		ros::spinOnce();
	}
	//if (command == MOVE_FORWARD_DIST)
	//  command = -1;
	//printf("STOP\n");
  std_msgs::Int32 pub;
  pub.data = 1;
  inter_Pub.publish(pub);
	motor.right = 0;
	motor.left = 0;
	for (int i = 0; i < 8; i++)
	{
	  mot_pub.publish(motor);
	  ros::spinOnce();
	}
	
}

/*
Turns the robot angle degrees to the right by turning the wheels 
in opposite directions.

As in the 'forward' function, keeping the robot on track is done by halting the
wheel which is ahead.
*/
void t_right(double angle, int sl , int sr){
	motor.right = -TURN_SPEED;
	motor.left = TURN_SPEED;
	mot_pub.publish(motor);
	
	
	ros::Rate loop(60);
  sl = l;
	sr = r;
	printf("START RIGHT...Angle: %f \n", angle);
	while(((dist(l, sl) + dist(r, sr))/2.0) < (angle)*TICKS_P_DEGREE){
	  
		if(dist(l, sl) != dist(r, sr)){
			if(dist(l, sl) < dist(r, sr)){
				motor.left = TURN_SPEED;
				motor.right = 0.9*-TURN_SPEED;
			}
			else{
				motor.right = -TURN_SPEED;
				motor.left = 0.9*TURN_SPEED;
			}
		}
		
		mot_pub.publish(motor);
	
		loop.sleep();
		ros::spinOnce();
		//printf("Distance used: %f, Distance expected: %f, Left distance: %f, Right distance: %f, Angle: %f\n", 
	  //        ((dist(l, sl) + dist(r, sr))/2.0), 
	   //       (angle)*TICKS_P_DEGREE, 
	    //      dist(l,sl),
	     //     dist(r,sr), angle);
	}
	//if (command == TURN_RIGHT)
  //	command = -1;
	printf("STOP\n");
  std_msgs::Int32 pub;
  pub.data = (1);
  inter_Pub.publish(pub);
	motor.right = 0;
	motor.left = 0;

	mot_pub.publish(motor);
  	
}

/*
Exactly like t_right function but turns the robot angle degress left.
*/
void t_left(double angle, int sl, int sr){
	motor.right = TURN_SPEED;
	motor.left = -TURN_SPEED;
	mot_pub.publish(motor);
	
	ros::Rate loop(60);
  sl = l;
  sr = r;
	printf("START LEFT... Angle: %f \n", angle);
	while(((dist(l, sl) + dist(r, sr))/2.0) < (angle*TICKS_P_DEGREE)){ //165 ~= 90 degree
		//usleep(50);
		
		if(dist(l, sl) != dist(r, sr)){
			if(dist(l, sl) < dist(r, sr)){
				//motor.left = -TURN_SPEED;
				//motor.right = 0.6*TURN_SPEED;
				motor.left = 0.9*-TURN_SPEED;
				motor.right = TURN_SPEED;
			}
			else{
				//motor.right = TURN_SPEED;
				//motor.left = 0.6*-TURN_SPEED;
				motor.right = 0.9*TURN_SPEED;
				motor.left = -TURN_SPEED;
			} 
		}
		mot_pub.publish(motor);
	
		loop.sleep();
		ros::spinOnce();
		//printf("Distance used: %f, Distance expected: %f, Left distance: %f, Right distance: %f, Angle: %f\n", 
	          //((dist(l, sl) + dist(r, sr))/2.0), 
	          //((angle)*TICKS_P_DEGREE), 
	          //dist(l,sl),
	          //dist(r,sr),
	          //angle);
	}
	//if (command == TURN_LEFT)
  //	command = -1;
	printf("STOP\n");
  std_msgs::Int32 pub;
  pub.data = (1);
  inter_Pub.publish(pub);
	motor.right = 0;
	motor.left = 0;
  
	mot_pub.publish(motor);

}

//LIKE A BOSS
	
